from math import pi
import time

from std_msgs.msg import Header
from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3
from moveit.task_constructor import core, stages

import rclcpp


def main():
    rclcpp.init()
    node = rclcpp.Node("mtc_tutorial_merger")

    joint_space = core.JointInterpolationPlanner()

    task = core.Task()
    task.name = "cartesian"
    task.loadRobotModel(node)

    # start from current robot state
    task.add(stages.CurrentState("current state"))
    # --------------------------------------------------------------------------
    merger1 = core.Merger("stand_open")

    moveTo11 = stages.MoveTo("Move To stand", joint_space)
    moveTo11.group = "arm"
    moveTo11.setGoal("stand_pose")
    merger1.insert(moveTo11)

    moveTo12 = stages.MoveTo("Move To Home", joint_space)
    moveTo12.group = "hand"
    moveTo12.setGoal("open_pose")
    merger1.insert(moveTo12)

    task.add(merger1)
    # --------------------------------------------------------------------------
    merger2 = core.Merger("rest_close")

    moveTo21 = stages.MoveTo("Move To rest", joint_space)
    moveTo21.group = "arm"
    moveTo21.setGoal("rest_pose")
    merger2.insert(moveTo21)

    moveTo22 = stages.MoveTo("Move To close", joint_space)
    moveTo22.group = "hand"
    moveTo22.setGoal("close_pose")
    merger2.insert(moveTo22)

    task.add(merger2)

    if task.plan():
        task.publish(task.solutions[0])

    time.sleep(100)
